#pragma once
#include "glut.h"
#include <highgui.h>
#include "ChessboardSurrogate.h"
#include "testSet.h"
#include "traceManager.h"

namespace camModule
{
	////////////////////////////////////////////////////////////////////////////////////////////////////////
	/// Varibles
	////////////////////////////////////////////////////////////////////////////////////////////////////////
	const int square_size = 1;

	testSet::testUnit* _data;
	traceManager tracer;

	CvCapture* _capture;
	CvSize _boardSize = cvSize(8, 6);
	CvPoint2D32f* _imagePointsBuf;
	int _cornersFound = 0;
	int _cornersCount = 0;
	CvMat* _object_points;
	CvMat* _r;
	CvMat* _t;

	int MouseActive = FALSE;
	float angleX = 0, angleY = 0;
	int lastX, lastY;

	////////////////////////////////////////////////////////////////////////////////////////////////////////
	void Idle();

	////////////////////////////////////////////////////////////////////////////////////////////////////////
	void keyboard(unsigned char key, int x, int y)
	{
	   switch (key) {
		  case 27:
			 exit(0);
			 break;
		  case 'w':
				Idle();
			  break;
		  case 'r':
				angleX = 0;
				angleY = 0;
				printf("restore default view position\r\n");
				glutPostRedisplay();
			  break;
	   }
	}

	void processMouse(int button, int state, int x, int y) 
	{
		if (state == GLUT_DOWN)
		{
			lastX = x;
			lastY = y;
		
			MouseActive = TRUE;			
		}
		else if (state == GLUT_UP)
		{
			MouseActive = FALSE;
		}
	}

	void processMouseActiveMotion(int x, int y) 
	{
		if (MouseActive)
		{
			int dx = lastX - x;
			int dy = lastY - y;

			if (dx < 0)
				angleY+=2;
			else if (dx > 0)
				angleY-=2;


			if (dy < 0)
				angleX+=2;
			else if (dy > 0)
				angleX-=2;

			lastX = x;
			lastY = y;
		}

		glutPostRedisplay();
	}

	void init() 
	{
		GLfloat front_color[] = {0,1,0,1};

	   GLfloat mat_specular[] = { 1.0, 1.0, 1.0, 1.0 };
	   GLfloat mat_shininess[] = { 50.0 };

	   GLfloat light_position[] = { 0.0, 0.0, -2000 };
	   GLfloat light[] = { 1.0, 0.2, 0.2 };
	   GLfloat lmodel_ambient[] = { 0.1, 0.1, 0.1, 1.0 };
	   glClearColor (0.0, 0.0, 0.0, 0.0);
	   glShadeModel (GL_SMOOTH);

	   glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
	   glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);

	   glLightfv(GL_LIGHT0, GL_POSITION, light_position);

	   glLightfv(GL_LIGHT0, GL_DIFFUSE, light );
	   glLightfv(GL_LIGHT0, GL_SPECULAR, light );
	   glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lmodel_ambient);

	   glEnable(GL_LIGHTING);
	   glEnable(GL_LIGHT0);
	   glEnable(GL_DEPTH_TEST);
	}

	void drawCoordinateSystem()
	{
		glBegin(GL_LINES);
			glColor3f(1.0f,0.0f,0.0f); //y is red
			glVertex3i(0,200,0);
			glVertex3i(0,-5,0);
			glColor3f(0.0f,1.0f,0.0f); //x is green
			glVertex3i(200,0,0);
			glVertex3i(-5,0,0);
			glColor3f(0.0f,0.0f,1.0f); //z is blue
			glVertex3i(0,0,200);
			glVertex3i(0,0,-5);
		glEnd();
	}

	void display()
	{
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		
		glPushMatrix();

			glMultMatrixf(_data->m);

			glRotatef(angleX,1.0,0.0,0.0);
			glRotatef(angleY,0.0,1.0,0.0);

			//glutSolidCube(100);
			//drawCoordinateSystem();		
			ChessboardSurrogate s(6,8,50);
			s.Draw(0,0);
		
		glPopMatrix();
		
		// and in the end show trace
		//tracer.showTrace();
		
		glutSwapBuffers();
	}

	void reshape(int w, int h)
	{
		glViewport(0, 0, w, h);
		
		glMatrixMode(GL_PROJECTION);
			glLoadIdentity();
			_data->setupFrustum(w,h);

		glMatrixMode(GL_MODELVIEW);

		glLoadIdentity();		
		gluLookAt(  0.0, 0.0, -testSet::znear*2,
			0.0, 0.0, 0.0,
			0.0, 1.0, 0.0);
	}

	int tmpImageCount = 0;
	void Idle()
	{
		int count;
		//IplImage* img = cvQueryFrame(_capture);
		
		char buff[256];
		sprintf(buff, "snapshots\\%i.png", tmpImageCount % 8);
		tmpImageCount++;

		IplImage* img = cvLoadImage(buff);
		DWORD t1 = GetTickCount();
		
		cvFindChessboardCorners(img, _boardSize,  _imagePointsBuf, &_cornersFound, CV_CALIB_CB_ADAPTIVE_THRESH);
		if (_cornersFound < _cornersCount )
			printf("less then %i corners found\r\n");
		else
		{			
			CvMat imagePoints = cvMat(1, _cornersFound, CV_32FC2, _imagePointsBuf);
			cvFindExtrinsicCameraParams2(_object_points, &imagePoints, &_data->Camera, &_data->Distortion, _r, _t);
			_data->SetRT(_r->data.fl,  _t->data.fl);
			tracer.push((CvPoint3D32f*) _t->data.fl);
		}

		printf("processing time: %i ms\r\n", GetTickCount() - t1);
		
		cvFlip(img);
		cvFlip(img,img, 1);
		cvShowImage("wnd", img);

		glutPostRedisplay();
	}

	void run(int *argcp, char **argv)
	{
		testSet::Init();
		_data = testSet::tests[3]; // generalizated calibration info

		_cornersCount = _boardSize.width*_boardSize.height;
		_imagePointsBuf = (CvPoint2D32f*)cvAlloc( _cornersCount*sizeof(_imagePointsBuf[0]));

		_object_points = cvCreateMat( 1, _cornersCount, CV_32FC3 );
		CvPoint3D32f* obj_pt = ((CvPoint3D32f*)_object_points->data.fl);
		for(int j = 0; j < _boardSize.height; j++ )
			for(int k = 0; k < _boardSize.width; k++ )
				*obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);

		_r = cvCreateMat(1, 3, CV_32F);
		_t = cvCreateMat(1, 3, CV_32F);

		cvNamedWindow("wnd");
		_capture = cvCaptureFromCAM(0);

		glutInit(argcp, argv);
		glutInitDisplayMode( GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
		glutCreateWindow("test");  
		glutPositionWindow(1400, 500);
		glutReshapeWindow(640, 480);

		init();

		glutDisplayFunc(display);
		glutReshapeFunc(reshape);

		glutKeyboardUpFunc(keyboard);
		glutMouseFunc(processMouse);
		glutMotionFunc(processMouseActiveMotion);

		//glutIdleFunc(Idle);
		
		glutMainLoop();	

		cvReleaseCapture(&_capture);
		cvFree(&_imagePointsBuf);
		cvReleaseMat(&_r);
		cvReleaseMat(&_t);
	}
}